{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 21,
   "id": "111bce49",
   "metadata": {},
   "outputs": [],
   "source": [
    "import time\n",
    "import math\n",
    "import numpy as np\n",
    "from numpy.linalg import norm\n",
    "import pinocchio as pin\n",
    "import example_robot_data as robex\n",
    "from scipy.optimize import fmin_bfgs\n",
    "\n",
    "import sys\n",
    "sys.path.append(\"/home/uwsl/workspace/humanoid/tps_robotics_ens_2024\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "id": "5a8b3fde",
   "metadata": {},
   "outputs": [],
   "source": [
    "import pinocchio\n",
    "from pinocchio.robot_wrapper import RobotWrapper\n",
    "\n",
    "\n",
    "urdf_path = \"/home/uwsl/workspace/humanoid/tps_robotics_ens_2024/fangnuo-armv1.0/g1/g1_29dof_rev_1_0.urdf\"\n",
    "package_dirs = \"/home/uwsl/workspace/humanoid/tps_robotics_ens_2024/fangnuo-armv1.0/g1\"\n",
    "robot = RobotWrapper.BuildFromURDF(urdf_path, package_dirs=package_dirs)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "id": "9f661c35",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Nb joints = 30 (nq=29,nv=29)\n",
      "  Joint 0 universe: parent=0\n",
      "  Joint 1 left_hip_pitch_joint: parent=0\n",
      "  Joint 2 left_hip_roll_joint: parent=1\n",
      "  Joint 3 left_hip_yaw_joint: parent=2\n",
      "  Joint 4 left_knee_joint: parent=3\n",
      "  Joint 5 left_ankle_pitch_joint: parent=4\n",
      "  Joint 6 left_ankle_roll_joint: parent=5\n",
      "  Joint 7 right_hip_pitch_joint: parent=0\n",
      "  Joint 8 right_hip_roll_joint: parent=7\n",
      "  Joint 9 right_hip_yaw_joint: parent=8\n",
      "  Joint 10 right_knee_joint: parent=9\n",
      "  Joint 11 right_ankle_pitch_joint: parent=10\n",
      "  Joint 12 right_ankle_roll_joint: parent=11\n",
      "  Joint 13 waist_yaw_joint: parent=0\n",
      "  Joint 14 waist_roll_joint: parent=13\n",
      "  Joint 15 waist_pitch_joint: parent=14\n",
      "  Joint 16 left_shoulder_pitch_joint: parent=15\n",
      "  Joint 17 left_shoulder_roll_joint: parent=16\n",
      "  Joint 18 left_shoulder_yaw_joint: parent=17\n",
      "  Joint 19 left_elbow_joint: parent=18\n",
      "  Joint 20 left_wrist_roll_joint: parent=19\n",
      "  Joint 21 left_wrist_pitch_joint: parent=20\n",
      "  Joint 22 left_wrist_yaw_joint: parent=21\n",
      "  Joint 23 right_shoulder_pitch_joint: parent=15\n",
      "  Joint 24 right_shoulder_roll_joint: parent=23\n",
      "  Joint 25 right_shoulder_yaw_joint: parent=24\n",
      "  Joint 26 right_elbow_joint: parent=25\n",
      "  Joint 27 right_wrist_roll_joint: parent=26\n",
      "  Joint 28 right_wrist_pitch_joint: parent=27\n",
      "  Joint 29 right_wrist_yaw_joint: parent=28\n",
      "\n",
      "[0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0. 0.\n",
      " 0. 0. 0. 0. 0.]\n"
     ]
    }
   ],
   "source": [
    "print(robot.model)\n",
    "print(robot.q0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "id": "72de0679",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0 universe\n",
      "1 left_hip_pitch_joint\n",
      "2 left_hip_roll_joint\n",
      "3 left_hip_yaw_joint\n",
      "4 left_knee_joint\n",
      "5 left_ankle_pitch_joint\n",
      "6 left_ankle_roll_joint\n",
      "7 right_hip_pitch_joint\n",
      "8 right_hip_roll_joint\n",
      "9 right_hip_yaw_joint\n",
      "10 right_knee_joint\n",
      "11 right_ankle_pitch_joint\n",
      "12 right_ankle_roll_joint\n",
      "13 waist_yaw_joint\n",
      "14 waist_roll_joint\n",
      "15 waist_pitch_joint\n",
      "16 left_shoulder_pitch_joint\n",
      "17 left_shoulder_roll_joint\n",
      "18 left_shoulder_yaw_joint\n",
      "19 left_elbow_joint\n",
      "20 left_wrist_roll_joint\n",
      "21 left_wrist_pitch_joint\n",
      "22 left_wrist_yaw_joint\n",
      "23 right_shoulder_pitch_joint\n",
      "24 right_shoulder_roll_joint\n",
      "25 right_shoulder_yaw_joint\n",
      "26 right_elbow_joint\n",
      "27 right_wrist_roll_joint\n",
      "28 right_wrist_pitch_joint\n",
      "29 right_wrist_yaw_joint\n"
     ]
    }
   ],
   "source": [
    "for i, n in enumerate(robot.model.names):\n",
    "    print(i, n)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "id": "b115bf2a",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "29"
      ]
     },
     "execution_count": 25,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "robot.index('right_wrist_yaw_joint')\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "id": "6f44ea17",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "universe attached to joint # 0\n",
      "root_joint attached to joint # 0\n",
      "pelvis attached to joint # 0\n",
      "imu_in_pelvis_joint attached to joint # 0\n",
      "imu_in_pelvis attached to joint # 0\n",
      "left_hip_pitch_joint attached to joint # 1\n",
      "left_hip_pitch_link attached to joint # 1\n",
      "left_hip_roll_joint attached to joint # 2\n",
      "left_hip_roll_link attached to joint # 2\n",
      "left_hip_yaw_joint attached to joint # 3\n",
      "left_hip_yaw_link attached to joint # 3\n",
      "left_knee_joint attached to joint # 4\n",
      "left_knee_link attached to joint # 4\n",
      "left_ankle_pitch_joint attached to joint # 5\n",
      "left_ankle_pitch_link attached to joint # 5\n",
      "left_ankle_roll_joint attached to joint # 6\n",
      "left_ankle_roll_link attached to joint # 6\n",
      "pelvis_contour_joint attached to joint # 0\n",
      "pelvis_contour_link attached to joint # 0\n",
      "right_hip_pitch_joint attached to joint # 7\n",
      "right_hip_pitch_link attached to joint # 7\n",
      "right_hip_roll_joint attached to joint # 8\n",
      "right_hip_roll_link attached to joint # 8\n",
      "right_hip_yaw_joint attached to joint # 9\n",
      "right_hip_yaw_link attached to joint # 9\n",
      "right_knee_joint attached to joint # 10\n",
      "right_knee_link attached to joint # 10\n",
      "right_ankle_pitch_joint attached to joint # 11\n",
      "right_ankle_pitch_link attached to joint # 11\n",
      "right_ankle_roll_joint attached to joint # 12\n",
      "right_ankle_roll_link attached to joint # 12\n",
      "waist_yaw_joint attached to joint # 13\n",
      "waist_yaw_link attached to joint # 13\n",
      "waist_roll_joint attached to joint # 14\n",
      "waist_roll_link attached to joint # 14\n",
      "waist_pitch_joint attached to joint # 15\n",
      "torso_link attached to joint # 15\n",
      "d435_joint attached to joint # 15\n",
      "d435_link attached to joint # 15\n",
      "head_joint attached to joint # 15\n",
      "head_link attached to joint # 15\n",
      "imu_in_torso_joint attached to joint # 15\n",
      "imu_in_torso attached to joint # 15\n",
      "left_shoulder_pitch_joint attached to joint # 16\n",
      "left_shoulder_pitch_link attached to joint # 16\n",
      "left_shoulder_roll_joint attached to joint # 17\n",
      "left_shoulder_roll_link attached to joint # 17\n",
      "left_shoulder_yaw_joint attached to joint # 18\n",
      "left_shoulder_yaw_link attached to joint # 18\n",
      "left_elbow_joint attached to joint # 19\n",
      "left_elbow_link attached to joint # 19\n",
      "left_wrist_roll_joint attached to joint # 20\n",
      "left_wrist_roll_link attached to joint # 20\n",
      "left_wrist_pitch_joint attached to joint # 21\n",
      "left_wrist_pitch_link attached to joint # 21\n",
      "left_wrist_yaw_joint attached to joint # 22\n",
      "left_wrist_yaw_link attached to joint # 22\n",
      "left_hand_palm_joint attached to joint # 22\n",
      "left_rubber_hand attached to joint # 22\n",
      "logo_joint attached to joint # 15\n",
      "logo_link attached to joint # 15\n",
      "mid360_joint attached to joint # 15\n",
      "mid360_link attached to joint # 15\n",
      "right_shoulder_pitch_joint attached to joint # 23\n",
      "right_shoulder_pitch_link attached to joint # 23\n",
      "right_shoulder_roll_joint attached to joint # 24\n",
      "right_shoulder_roll_link attached to joint # 24\n",
      "right_shoulder_yaw_joint attached to joint # 25\n",
      "right_shoulder_yaw_link attached to joint # 25\n",
      "right_elbow_joint attached to joint # 26\n",
      "right_elbow_link attached to joint # 26\n",
      "right_wrist_roll_joint attached to joint # 27\n",
      "right_wrist_roll_link attached to joint # 27\n",
      "right_wrist_pitch_joint attached to joint # 28\n",
      "right_wrist_pitch_link attached to joint # 28\n",
      "right_wrist_yaw_joint attached to joint # 29\n",
      "right_wrist_yaw_link attached to joint # 29\n",
      "right_hand_palm_joint attached to joint # 29\n",
      "right_rubber_hand attached to joint # 29\n"
     ]
    }
   ],
   "source": [
    "for f in robot.model.frames:\n",
    "    print(f.name, 'attached to joint #', f.parent)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a29a82ac",
   "metadata": {},
   "outputs": [],
   "source": [
    "from utils.meshcat_viewer_wrapper import MeshcatVisualizer, colors  # noqa: E402\n",
    "\n",
    "viz = MeshcatVisualizer(robot)\n",
    "viz.viewer.jupyter_cell()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "robotics_course",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.17"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
